
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       location.c
  * @author     baiyang
  * @date       2021-7-7
  * @note       地理坐标系下的位置（经纬度），高度信息此模块中定义了4中坐标
  *         绝对坐标、相对home点、相对原点、相对地形。
  *         绝对坐标：绝对气压高
  *         home点：解锁点的GPS高程
  *         原点：飞控初始上电时的经纬高，GPS定位下高度是GPS高程，无GPS定位
  *               是气压计测得的相对标准海平面的高度
  *         地形：地形文件中记录的高度
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <string.h>
#include <stdbool.h>
#include <stdlib.h>

#include "location.h"

#include <ahrs/ahrs_view.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
const Location definitely_zero = {0};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       判断位置结构体是不是零
  * @param[in]   tLocation  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_is_zero(Location *tLocation)
{
    return !memcmp(tLocation, &definitely_zero, sizeof(*tLocation));
}

/**
  * @brief       清零位置结构体
  * @param[in]   tLocation  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_zero(Location *tLocation)
{
    memset(tLocation, 0, sizeof(*tLocation));
}

/**
  * @brief       设置位置点高度
  * @param[in]   tLocation  
  * @param[in]   alt_cm  
  * @param[in]   frame  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_set_alt_cm(Location *tLocation, int32_t alt_cm, AltFrame frame)
{
    tLocation->alt = alt_cm;
    tLocation->tflags.relative_alt = false;
    tLocation->tflags.terrain_alt = false;
    tLocation->tflags.origin_alt = false;
    switch (frame) {
        case ALT_FRAME_ABSOLUTE:
            // do nothing
            break;
        case ALT_FRAME_ABOVE_HOME:
            tLocation->tflags.relative_alt = true;
            break;
        case ALT_FRAME_ABOVE_ORIGIN:
            tLocation->tflags.origin_alt = true;
            break;
        case ALT_FRAME_ABOVE_TERRAIN:
            // we mark it as a relative altitude, as it doesn't have
            // home alt added
            tLocation->tflags.relative_alt = true;
            tLocation->tflags.terrain_alt = true;
            break;
    }
}

/**
  * @brief       设置位置点经纬度和高度
  * @param[in]   tLocation  
  * @param[in]   alt_cm  
  * @param[in]   frame  
  * @param[out]  
  * @retval      
  * @note        
  */

void location_from_lat_lon_alt(Location *tLocation, int32_t latitude, int32_t longitude, int32_t alt_in_cm, AltFrame frame)
{
    location_zero(tLocation);

    tLocation->lat = latitude;
    tLocation->lng = longitude;

    location_set_alt_cm(tLocation,alt_in_cm, frame);
}

/**
  * @brief       获取高度的坐标系
  * @param[in]   tLocation  
  * @param[out]  
  * @retval      
  * @note        
  */
AltFrame location_get_alt_frame(const Location *tLocation)
{
    if (tLocation->tflags.terrain_alt) {
        return ALT_FRAME_ABOVE_TERRAIN;
    }
    if (tLocation->tflags.origin_alt) {
        return ALT_FRAME_ABOVE_ORIGIN;
    }
    if (tLocation->tflags.relative_alt) {
        return ALT_FRAME_ABOVE_HOME;
    }
    return ALT_FRAME_ABSOLUTE;
}

/**
  * @brief       获取期望坐标下的高度
  * @param[in]   tLocation  
  * @param[in]   desired_frame  
  * @param[in]   ret_alt_cm  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_get_alt_cm(const Location *tLocation, AltFrame desired_frame, int32_t *ret_alt_cm)
{
    AltFrame frame = location_get_alt_frame(tLocation);

    ahrs_view* ahrs = get_ahrs_view();

    // shortcut if desired and underlying frame are the same
    if (desired_frame == frame) {
        *ret_alt_cm = tLocation->alt;
        return true;
    }

    // check for terrain altitude
    float alt_terr_cm = 0;
    if (frame == ALT_FRAME_ABOVE_TERRAIN || desired_frame == ALT_FRAME_ABOVE_TERRAIN) {
        return false;
    }

    // convert alt to absolute
    int32_t alt_abs = 0;
    switch (frame) {
        case ALT_FRAME_ABSOLUTE:
            alt_abs = tLocation->alt;
            break;
        case ALT_FRAME_ABOVE_HOME:
            if (!ahrs->home_valid_alt) {
                return false;
            }
            alt_abs = tLocation->alt + ahrs->home_alt_amsl * 100.0f;
            break;
        case ALT_FRAME_ABOVE_ORIGIN:
            {
                // fail if we cannot get ekf origin
                if (!ahrs->valid_alt) {
                    return false;
                }
                alt_abs = tLocation->alt + ahrs->origin_alt_amsl*100.0f;
            }
            break;
        case ALT_FRAME_ABOVE_TERRAIN:
            alt_abs = tLocation->alt + alt_terr_cm;
            break;
    }

    // convert absolute to desired frame
    switch (desired_frame) {
        case ALT_FRAME_ABSOLUTE:
            *ret_alt_cm = alt_abs;
            return true;
        case ALT_FRAME_ABOVE_HOME:
            if (!ahrs->home_valid_alt) {
                return false;
            }
            *ret_alt_cm = alt_abs - ahrs->home_alt_amsl * 100.0f;
            return true;
        case ALT_FRAME_ABOVE_ORIGIN:
            {
                // fail if we cannot get ekf origin
                if (!ahrs->valid_alt) {
                    return false;
                }
                *ret_alt_cm = alt_abs - ahrs->origin_alt_amsl*100.0f;
                return true;
            }
        case ALT_FRAME_ABOVE_TERRAIN:
            *ret_alt_cm = alt_abs - alt_terr_cm;
            return true;
    }
    return false;
}

/**
  * @brief       将高度值转换到新的坐标系下
  * @param[in]   tLocation  
  * @param[in]   desired_frame  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_change_alt_frame(Location *tLocation, AltFrame desired_frame)
{
    int32_t new_alt_cm;

    if (!location_get_alt_cm(tLocation,desired_frame, &new_alt_cm)) {
        return false;
    }

    location_set_alt_cm(tLocation, new_alt_cm, desired_frame);

    return true;
}

float location_longitude_scale(const Location *tLocation)
{
    float scale = cosf(tLocation->lat * (1.0e-7f * DEG_TO_RAD));
    return MAX(scale, 0.01f);
}

float location_longitude_scale2(int32_t lat)
{
    float scale = cosf(lat * (1.0e-7f * DEG_TO_RAD));
    return MAX(scale, 0.01f);
}

/**
  * @brief       返回北东坐标系下的，以tLocation为原点，loc2为目标点的矢量
  * @param[in]   tLocation  
  * @param[in]   vec_ne  
  * @param[out]  
  * @retval      
  * @note        单位：厘米
  */
bool location_get_vector_xy_from_origin_ne(const Location *tLocation, Vector2f_t *vec_ne)
{
    ahrs_view* ahrs = get_ahrs_view();

    if (!ahrs->valid_alt)
    {
        return false;
    }

    vec_ne->x = (tLocation->lat-ahrs->origin_lat) * LATLON_TO_CM;
    vec_ne->y = (tLocation->lng-ahrs->origin_lng) * LATLON_TO_CM * location_longitude_scale2(ahrs->origin_lat);

    return true;
}

/**
  * @brief       返回北东天坐标系下的，以tLocation为原点，loc2为目标点的矢量
  * @param[in]   tLocation  
  * @param[in]   vec_neu  
  * @param[out]  
  * @retval      
  * @note        单位：厘米
  */
bool location_get_vector_from_origin_neu(Location *tLocation, Vector3f_t *vec_neu)
{
    // convert lat, lon
    Vector2f_t vec_ne;
    if (!location_get_vector_xy_from_origin_ne(tLocation,&vec_ne)) {
        return false;
    }

    vec_neu->x = vec_ne.x;
    vec_neu->y = vec_ne.y;

    // convert altitude
    int32_t alt_above_origin_cm = 0;
    if (!location_get_alt_cm(tLocation,ALT_FRAME_ABOVE_ORIGIN, &alt_above_origin_cm)) {
        return false;
    }
    vec_neu->z = alt_above_origin_cm;

    return true;
}

/**
  * @brief       返回两个位置点之间的距离
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        单位：米
  */
float location_get_distance(Location *tLocation, const Location *loc2)
{
    float dlat = (float)(loc2->lat - tLocation->lat);
    float dlng = ((float)(loc2->lng - tLocation->lng)) * location_longitude_scale(loc2);

    return sqrtf(dlat*dlat+dlng*dlng) * LOCATION_SCALING_FACTOR;
}

/**
  * @brief       返回北东坐标系下的，以tLocation为原点，loc2为目标点的矢量
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector2f_t location_get_distance_ne(const Location *tLocation, const Location *loc2)
{
    Vector2f_t tLocNE = {(loc2->lat - tLocation->lat) * LOCATION_SCALING_FACTOR,
            (loc2->lng - tLocation->lng) * LOCATION_SCALING_FACTOR * location_longitude_scale(tLocation)};

    return tLocNE;
}

/**
  * @brief       返回北东地坐标系下的，以tLocation为原点，loc2为目标点的矢量
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        
  */
Vector3f_t location_get_distance_ned(const Location *tLocation, const Location *loc2)
{
    Vector3f_t tLocNED = {(loc2->lat - tLocation->lat) * LOCATION_SCALING_FACTOR,
                (loc2->lng - tLocation->lng) * LOCATION_SCALING_FACTOR * location_longitude_scale(tLocation),
                (tLocation->alt - loc2->alt) * 0.01f};

    return tLocNED;
}

/**
  * @brief       根据北和东给定距离（以米为单位），推算纬度/经度
  * @param[in]   tLocation  
  * @param[in]   ofs_north  
  * @param[in]   ofs_east  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_offset(Location *tLocation, float ofs_north, float ofs_east)
{
    const int32_t dlat = ofs_north * LOCATION_SCALING_FACTOR_INV;
    const int32_t dlng = (ofs_east * LOCATION_SCALING_FACTOR_INV) / location_longitude_scale(tLocation);

    tLocation->lat += dlat;
    tLocation->lng += dlng;
}

/**
  * @brief       在给定方位角和距离的情况下外推纬度/经度
  * @param[in]   tLocation  
  * @param[in]   bearing  
  * @param[in]   distance  
  * @param[out]  
  * @retval      
  * @note        请注意，此功能在100m的距离处精确到大约1mm。
  *              此功能的优点是可以在相对位置工作，因此即使在处理小距离和浮点数时也可以保持精度
  */
void location_offset_bearing(Location *tLocation, float bearing, float distance)
{
    const float ofs_north = cosf(Deg2Rad(bearing)) * distance;
    const float ofs_east  = sinf(Deg2Rad(bearing)) * distance;

    location_offset(tLocation, ofs_north, ofs_east);
}

/**
  * @brief       根据给定的方位，螺距和距离推断纬度/经度
  * @param[in]   tLocation  
  * @param[in]   bearing  
  * @param[in]   pitch  
  * @param[in]   distance  
  * @param[out]  
  * @retval      
  * @note        
  */
void location_offset_bearing_and_pitch(Location *tLocation, float bearing, float pitch, float distance)
{
    const float ofs_north =  cosf(Deg2Rad(pitch)) * cosf(Deg2Rad(bearing)) * distance;
    const float ofs_east  =  cosf(Deg2Rad(pitch)) * sinf(Deg2Rad(bearing)) * distance;

    location_offset(tLocation, ofs_north, ofs_east);

    const int32_t dalt =  sinf(Deg2Rad(pitch)) * distance *100.0f;
    tLocation->alt += dalt; 
}

bool location_check_lat(int32_t lat)
{
    return labs(lat) <= 90*1e7;
}
bool location_check_lng(int32_t lng)
{
    return labs(lng) <= 180*1e7;
}

/**
  * @brief       当lat和lng在范围内时，返回true
  * @param[in]   tLocation  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_check_lat_lng(Location *tLocation)
{
    return location_check_lat(tLocation->lat) && location_check_lng(tLocation->lng);
}

/**
  * @brief       用有用的数据转换无效的航点。如果位置点被更改，则返回true
  * @param[in]   tLocation  
  * @param[in]   defaultLoc  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_sanitize(Location *tLocation, const Location *defaultLoc)
{
    bool has_changed = false;
    // convert lat/lng=0 to mean current point
    if (tLocation->lat == 0 && tLocation->lng == 0) {
        tLocation->lat = defaultLoc->lat;
        tLocation->lng = defaultLoc->lng;
        has_changed = true;
    }

    // convert relative alt=0 to mean current alt
    if (tLocation->alt == 0 && tLocation->tflags.relative_alt) {
        tLocation->tflags.relative_alt = false;
        tLocation->alt = defaultLoc->alt;
        has_changed = true;
    }

    // limit lat/lng to appropriate ranges
    if (!location_check_lat_lng(tLocation)) {
        tLocation->lat = defaultLoc->lat;
        tLocation->lng = defaultLoc->lng;
        has_changed = true;
    }

    return has_changed;
}

/**
  * @brief       点loc2在点tLocation北偏东的角度
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        单位：度*100
  */
int32_t location_get_bearing_to(Location *tLocation, const Location *loc2)
{
    const int32_t off_x = loc2->lng - tLocation->lng;
    const int32_t off_y = (loc2->lat - tLocation->lat) / location_longitude_scale(loc2);

    int32_t bearing = 9000 + atan2f(-off_y, off_x) * DEGX100;
    if (bearing < 0) {
        bearing += 36000;
    }

    return bearing;
}

/**
  * @brief       如果经纬度相同，则返回true
  * @param[in]   tLocation  
  * @param[in]   loc2  
  * @param[out]  
  * @retval      
  * @note        
  */
bool location_same_lat_lon_as(Location *tLocation, const Location *loc2)
{
    return (tLocation->lat == loc2->lat) && (tLocation->lng == loc2->lng);
}

/**
  * @brief       返回点tLocation在线段point1<->point2投影，
  *              与点point1的长度和point1<->point2长度的比例
  * @param[in]   tLocation  
  * @param[in]   point1  
  * @param[in]   point2  
  * @param[out]  
  * @retval      
  * @note        point1为线段起点，point2为线段终点
  */
float location_line_path_proportion(Location *tLocation, const Location *point1, const Location *point2)
{
    const Vector2f_t vec1 = location_get_distance_ne(point1,point2);
    const Vector2f_t vec2 = location_get_distance_ne(point1,tLocation);

    const float dsquared = vec1.x*vec1.x + vec1.y*vec1.y;
    if (dsquared < 0.001f) {
        // the two points are very close together
        return 1.0f;
    }

    return vec2_dot(&vec1,&vec2) / dsquared;
}

/**
  * @brief       如果点tLocation在线段point1->point2的投影，
  *              落在线段point1->point2外，且在point2一侧,则返回true
  * @param[in]   tLocation  
  * @param[in]   point1  
  * @param[in]   point2  
  * @param[out]  
  * @retval      
  * @note        point1为线段起点，point2为线段终点
  */
bool location_past_interval_finish_line(Location *tLocation, const Location *point1, const Location *point2)
{
    return location_line_path_proportion(tLocation, point1, point2) >= 1.0f;
}

/*------------------------------------test------------------------------------*/


